#include "MBMatrix4x4.h"

#include <glm/gtc/type_ptr.hpp>
#include <glm/gtc/matrix_transform.hpp>
#include <glm/gtx/transform.hpp>

#include <iostream>
#include <iomanip>

using std::cout;
using std::endl;
using std::cout;
using std::setw;

/**
  *	Constructs a new MBMatrix4x4 object with one value at every element on the
  * diagonal. All other values are 0.
  *
  * @param diagonalElem value that is used at every element on the diagonal.
  */
MBMatrix4x4::MBMatrix4x4( const double &diagonalElem ) :
	mMatrix( diagonalElem ) {};

/**
  *	Constructs a new MBMatrix4x4 with the 16 values of the given array
  *
  * @param values const pointer to a double array containing the 16 numbers of the matrix line by line
  */
MBMatrix4x4::MBMatrix4x4( const double *values ) :
	mMatrix (	values[0],  values[4],  values[8],  values[12],
				values[1],  values[5],  values[9],  values[13],
				values[2],  values[6],  values[10], values[14],
				values[3],  values[7],  values[11], values[15] ) {};

/**
  *	Constructs a new MBMatrix4x4 object with the given glm::mat4 matrix
  *
  * @param matrix glm::mat4 matrix with the values of the new MBMatrix4x4 object
  */
MBMatrix4x4::MBMatrix4x4( const glm::mat4 &matrix ) : mMatrix( matrix ) {};

/**
  *	Constructs a new MBMatrix4x4 with the 16 given values
  *
  * @param v11 value at row 1, column 1
  * @param v12 value at row 1, column 2
  * @param v13 value at row 1, column 3
  * @param v14 value at row 1, column 4
  * @param v21 value at row 2, column 1
  * @param v22 value at row 2, column 2
  * @param v23 value at row 2, column 3
  * @param v24 value at row 2, column 4
  * @param v31 value at row 3, column 1
  * @param v32 value at row 3, column 2
  * @param v33 value at row 3, column 3
  * @param v34 value at row 3, column 4
  * @param v41 value at row 4, column 1
  * @param v42 value at row 4, column 2
  * @param v43 value at row 4, column 3
  * @param v44 value at row 4, column 4
  */
MBMatrix4x4::MBMatrix4x4( const double &v11, const double &v12, const double &v13, const double &v14, 
						  const double &v21, const double &v22, const double &v23, const double &v24, 
						  const double &v31, const double &v32, const double &v33, const double &v34, 
						  const double &v41, const double &v42, const double &v43, const double &v44 ) :
	mMatrix (	v11, v21, v31, v41,
				v12, v22, v32, v42,
				v13, v23, v33, v43,
				v14, v24, v34, v44 ) {};

/**
  *	Returns the value at the given row and column
  *
  * @param row row of the element that is returned
  * @param col column of the element that is returned
  * @return value at the given row and column
  */
double &MBMatrix4x4::At( const MBuint &row, const MBuint &col )
{
	return mMatrix[col][row];
}

/**
  *	Returns the value at the given row and column
  *
  * @param row row of the element that is returned
  * @param col column of the element that is returned
  * @return value at the given row and column
  */
const double &MBMatrix4x4::At( const MBuint &row, const MBuint &col ) const
{
	return mMatrix[col][row];
}

/**
  *	Returns the inverse of this matrix
  *
  * @return inverse of this matrix
  */
MBMatrix4x4 MBMatrix4x4::GetInverse( void ) const
{
	return MBMatrix4x4( glm::inverse( mMatrix ) );
}

/**
  *	Returns the transpose of this matrix
  *
  * @return transpose of this matrix
  */
MBMatrix4x4 MBMatrix4x4::GetTranspose( void ) const
{
	return MBMatrix4x4( glm::transpose( mMatrix ) );
}

/**
  *	Prints the matrix to the console
  */
void MBMatrix4x4::Print( void ) const
{
	for (MBuint row = 0; row < 4; row++)
	{
		for (MBuint col = 0; col < 4; col++)
		{
			cout << setw( 15 ) << mMatrix[col][row];
		}
		cout << endl;
	}
	cout << endl;
}

/**
  *	Left-multiplies this matrix with another one and returns the result. This matrix
  * is not modified by this operation.
  *
  * @param other matrix that is right-multiplied to this one
  * @return product of this matrix and the other matrix
  */
MBMatrix4x4 MBMatrix4x4::operator*( const MBMatrix4x4 &other ) const
{
	return MBMatrix4x4( mMatrix * other.mMatrix );
}

/**
  *	Multiplies the given vector and this matrix and returns the result. The
  * result is calculated by: res = matrix * vec
  *
  * @param vec vector to be multiplied by the matrix
  * @return matrix * vec
  */
MBVec4 MBMatrix4x4::operator*( const MBVec4 &vec ) const
{
	return MBVec4( mMatrix[0][0] * vec.mValues[0] + mMatrix[1][0] * vec.mValues[1] + mMatrix[2][0] * vec.mValues[2] + mMatrix[3][0] * vec.mValues[3],
				   mMatrix[0][1] * vec.mValues[0] + mMatrix[1][1] * vec.mValues[1] + mMatrix[2][1] * vec.mValues[2] + mMatrix[3][1] * vec.mValues[3],
				   mMatrix[0][2] * vec.mValues[0] + mMatrix[1][2] * vec.mValues[1] + mMatrix[2][2] * vec.mValues[2] + mMatrix[3][2] * vec.mValues[3],
				   mMatrix[0][3] * vec.mValues[0] + mMatrix[1][3] * vec.mValues[1] + mMatrix[2][3] * vec.mValues[2] + mMatrix[3][3] * vec.mValues[3] );
}

/**
  *	Returns an MBMatrix4x4 object that contains the final Euler XYZ rotation
  * that is defined by the given arguments
  *
  * @param rotX rotation around the X-axis in degrees
  * @param rotY rotation around the Y-axis in degrees
  * @param rotZ rotation around the Z-axis in degrees
  * @return final Euler XYZ rotation
  */
MBMatrix4x4 MBMatrix4x4::RotationXYZ( const double &rotX, const double &rotY, const double &rotZ )
{
	glm::mat4 rot_mat_x = glm::rotate( rotX, glm::vec3( 1.0, 0.0, 0.0 ) );
	glm::mat4 rot_mat_y = glm::rotate( rotY, glm::vec3( 0.0, 1.0, 0.0 ) );
	glm::mat4 rot_mat_z = glm::rotate( rotZ, glm::vec3( 0.0, 0.0, 1.0 ) );

	return MBMatrix4x4( rot_mat_z * rot_mat_y * rot_mat_x );
}

/**
  *	Returns an MBMatrix4x4 object that contains the final translation
  * that is defined by the given arguments
  *
  * @param transX translation along the X-axis
  * @param transY translation along the Y-axis
  * @param transZ translation along the Z-axis
  * @return final translation
  */
MBMatrix4x4 MBMatrix4x4::TranslationXYZ( const double &transX, const double &transY, const double &transZ )
{
	glm::mat4 result( 1.0 );

	result = glm::translate( result, glm::vec3( transX, transY, transZ ) );

	return MBMatrix4x4( result );
}